#ifndef LOCATION_H
#define LOCATION_H

#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <opencv/cv.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/cxcore.h>
#include <math.h>
#include <iomanip>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>

using namespace std;
using namespace cv;
using namespace Eigen;

class Location
{
public:
  Location();
  ~Location();
  struct bucket_info
  {
      float dis_cam_bucket;
      vector<int> sum_appear;
      float rate;
      int frame_bucket;
      Point2f coordinate_info;
      vector<Point2f> coordinate;
      int lastframe_size;
      int frame;
  };
  struct AreaGrid{
    visualization_msgs::Marker circle;
  };
  void detectonReset_callback(const std_msgs::Int32 msg);
  void compute_dis2(vector<Eigen::VectorXf> model_circle,vector<Vector4d> &pose_laser,vector<bucket_info> &buckets_information);
  void pose_generation(vector<Eigen::VectorXf> &model_circle,Isometry3d t1, vector<Vector4d> &pose_laser);
  void rate_appear(Isometry3d t1,vector<bucket_info> &buckets_information,vector<Point2f> &location);
  void follow_location(vector<Vector4d> &pose_laser1,vector<Eigen::VectorXf> &model_circle, Isometry3d t1, vector<Point2f> &location,vector<Eigen::Vector4d> &pose_array,int detection_reset);
  void areaVisualization(vector<Eigen::Vector4d> &pose_array,AreaGrid& area_visual,vector<visualization_msgs::Marker>& circles_rviz);
  int detection_reset_=0;
private:
    vector<int> frame;
    vector<bucket_info> buckets_information;
    vector<Point2f> location;

    ros::Subscriber reset_sub_;
    ros::NodeHandle nh_;  ///< 全局句柄
};
#endif /* LOCATION_H */
